#include "PolyLine.h"

namespace poly
{
	PolyLine PolyLine::transform( float44 const & mat ) const
	{
		PolyLine out;
		for( size_t i = 0; i < size(); i++ )
		{
			out.push_back( transformCoord( (*this)[i], mat ) );
		}

		return out;
	}

	float PolyLine::length() const
	{
		float out = 0;
		for( size_t i = 0; i < size() - 1; i++ )
		{
			out += poly::length( (*this)[i+1] - (*this)[i] );
		}

		return out;
	}

	PolyLine::ClosestPointDesc PolyLine::closestPoint( float3 const & pos )
	{
		PolyLine::ClosestPointDesc closest;
		closest.point = (*this)[0];
		float seperationSq = lengthSq((*this)[0]-pos);
		for( size_t i = 0; i < size() - 1; i++ )
		{
			float3 dir = (*this)[i+1]-(*this)[i+0];
			float3 toPoint0 = pos - (*this)[i+0];
			float3 toPoint1 = pos - (*this)[i+1];

			// check if the closest point is on the segment
			float dot0 = dot(dir, toPoint0);
			float dot1 = dot(dir, toPoint1);

			if( dot0 > 0 && dot1 < 0 )
			{
				float3 projected = (*this)[i+0] + dot0 / lengthSq(dir) * dir;
				if( lengthSq(projected - pos) < seperationSq )
				{
					closest.point = projected;
					seperationSq = lengthSq(closest.point - pos);
					closest.type = closest.SEGMENT;
					closest.tangent = (*this)[i+1] - (*this)[i+0];
				}
			}
			else
				// check if closest point is the second vertex of this segment
				if( lengthSq(toPoint1) < seperationSq )
				{
					closest.point = (*this)[i+1];
					seperationSq = lengthSq(toPoint1);
					closest.type = closest.VERTEX;
					closest.tangent = zero3();
				}
		}

		return closest;
	}
}
